
/**
 * @file ClockJump.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2020/04/05
//
//  clock jump detection and repair, shjzhang
//
//============================================================================


#include "ClockJump.hpp"
#include "SystemTime.hpp"
#include "Ambiguity.hpp"
#include "NEUUtil.hpp"

#define debug 0

using namespace gpstk::StringUtils;

namespace gpstk
{

      // Index initially assigned to this class
   int ClockJump::classIndex = 9600000;

      // Returns an index identifying this object.
   int ClockJump::getIndex() const
   { return index; }

      // Returns a string identifying this object.
   std::string ClockJump::getClassName() const
   { return "ClockJump"; }

   void ClockJump::Init(void)
   {
       // set up variables and equations for PPP

       // Warning: not consider the GLONASS system until now
       // Warning: only consider LC combinations
       // Warning: see ClockJumpUC for UC combinations

       //////////////////////////////////////////////////////////////
       //
       // please augument for GAL/BDS system
       // 
       // wavelengthBLC12 = clight/(f1+f2)
       //
       // BLC12  = clight/(f1+f2)[( B1 + f2/(f1-f2)*Nw]
       // BLC15  = clight/(f1+f5)[( B1 + f5/(f1-f5)*Nw)]
       //
       //////////////////////////////////////////////////////////////

   }

      /* Code to be executed before 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& ClockJump::Process( gnssRinex& gRin)
       noexcept(false)
   {
      try
      {
          if(debug)
          {
              cout << "ClockJump" << endl;
              cout << YDSTime(gRin.header.epoch) << endl;
          }

          // let's compute the time-difference data for cycle-slip detection
          getEpochDiffData(gRin);


          // keep only the data which is used in the time-difference xyzt esimate
          TypeIDSet typeSet;

          // GPS
          typeSet.insert(TypeID::diffPrefitC12);
          typeSet.insert(TypeID::diffPrefitC15);

          // GAL
          typeSet.insert(TypeID::diffPrefitC15);
          typeSet.insert(TypeID::diffPrefitC17);
          typeSet.insert(TypeID::diffPrefitC18);
          typeSet.insert(TypeID::diffPrefitC16);

          // BDS
          typeSet.insert(TypeID::diffPrefitC27);
          typeSet.insert(TypeID::diffPrefitC26);

          // GPS
          typeSet.insert(TypeID::diffPrefitL12);
          typeSet.insert(TypeID::diffPrefitL15);

          // GAL
          typeSet.insert(TypeID::diffPrefitL15);
          typeSet.insert(TypeID::diffPrefitL17);
          typeSet.insert(TypeID::diffPrefitL18);
          typeSet.insert(TypeID::diffPrefitL16);

          // BDS
          typeSet.insert(TypeID::diffPrefitL27);
          typeSet.insert(TypeID::diffPrefitL26);

          gnssRinex gBack = gRin;
          gBack.keepOnlyTypeID(typeSet);

          if(firstTime)
          {
              firstTime = false;
              // store the satTypeValueMap
              stvDataPrev = gRin.body;
              return gRin;
          }

          // now, let's compute the codeClockJump for carrier-phase using
          // time-differenced ionosphere-free Geometry-free combinations
          double clockJump(0.0);
          clockJump = getClockJump(gRin);

          if(debug)
          cout << "clockJump" << clockJump << endl;

          // codeClockJump happened
          if( clockJump > 0)
          {

              // now, let's correct all code/phase observables
              for(auto& stv: gRin.body)
              {
                  // only correct phase clock jump
                  if(stv.second.find(TypeID::L1) != stv.second.end())
                  {
                      stv.second[TypeID::L1] -= clockJump;
                  }

                  if(stv.second.find(TypeID::L2) != stv.second.end())
                  {
                      stv.second[TypeID::L2] -= clockJump;
                  }

                  if(stv.second.find(TypeID::L5) != stv.second.end())
                  {
                      stv.second[TypeID::L5] -= clockJump;
                  }

                  if(stv.second.find(TypeID::L7) != stv.second.end())
                  {
                      stv.second[TypeID::L7] -= clockJump;
                  }

                  if(stv.second.find(TypeID::L8) != stv.second.end())
                  {
                      stv.second[TypeID::L8] -= clockJump;
                  }

                  if(stv.second.find(TypeID::L6) != stv.second.end())
                  {
                      stv.second[TypeID::L6] -= clockJump;
                  }

                  //////////////////////////// 
                  //
                  // the prefitC12/prefitL12 MUST be updated and stored for the
                  // correction in the next epoch
                  //
                  //////////////////////////// 
                  if(stv.second.find(TypeID::prefitL12) != stv.second.end())
                  {
                      stv.second[TypeID::prefitL12] -= clockJump;
                  }
              }
          }

          // store the satTypeValueMap
          stvDataPrev = gRin.body;

          // reset filter
          CommonTime epoch( gRin.header.epoch );

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'ClockJump::preCompute()'


   double ClockJump::getClockJump(gnssRinex& gRin)
   {

       // gps satSet
       SatIDSet gpsSatSet = gRin.getSatID(SatelliteSystem::GPS);

       Vector<double> diffPrefitL12Vec 
           = gRin.getVectorOfTypeID(TypeID::diffPrefitL12, SatelliteSystem::GPS);

       Vector<double> diffPrefitC12Vec 
           = gRin.getVectorOfTypeID(TypeID::diffPrefitC12, SatelliteSystem::GPS);

       Vector<double> csVec
           = gRin.getVectorOfTypeID(TypeID::CSFlag, SatelliteSystem::GPS);
       

       if(debug)
       {
           cout << "diffPrefitL12Vec" << endl;
           cout <<  diffPrefitL12Vec  << endl;

           cout << "diffPrefitC12Vec" << endl;
           cout <<  diffPrefitC12Vec  << endl;
       }

       if(diffPrefitL12Vec.size() != diffPrefitC12Vec.size())
       {
           cout << "diffPrefitL12 size don't equal with diffPrefitC12Vec" << endl;
           exit(-1);
       }

       int numSat = diffPrefitL12Vec.size();

       // now, let's compute the clock difference between diffPrefitL12Vec and diffPrefitC12Vec
       Vector<double> jumpVec(numSat, 0.0);

       double sum=0.0;
       int numJump = 0;
       for(int i=0; i<numSat; i++)
       {
           double cjValue = diffPrefitL12Vec[i] - diffPrefitC12Vec[i];
           if(std::abs(cjValue) > (jumpThreshold - 3.0*sigma))
           {
               jumpVec(i) = 1.0;
               sum += cjValue;
               numJump ++;
           }
       }

       // robust median clock jump value
       double clockJump(0.0);
       if(numJump > 0)
       {
          clockJump = std::floor(sum/(numJump*jumpThreshold) + 0.5) * jumpThreshold;
       }
       else
       {
          clockJump = 0.0;
       }

       return clockJump;

   };

   void ClockJump::getEpochDiffData(gnssRinex& gRin)
       noexcept(false)
   {
       try
       {
          // loop the satellite
          for(auto& stv: gRin.body)
          {
              SatID sat = stv.first;

              if(stvDataPrev.find(sat) != stvDataPrev.end())
              {
                  typeValueMap tvDataPrev = stvDataPrev[sat];
                  
                  if(sat.system == SatelliteSystem::GPS )
                  {
                      for(auto type: gpsCombTypeSet)
                      {
                          if(type==TypeID::LC12)
                          {
                              bool hasCurrPrefit(false);
                              if( stv.second.find(TypeID::prefitC12) != stv.second.end() &&
                                  stv.second.find(TypeID::prefitL12) != stv.second.end() )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( 
                                  tvDataPrev.find(TypeID::prefitC12) != tvDataPrev.end() &&
                                  tvDataPrev.find(TypeID::prefitL12) != tvDataPrev.end() 
                                      )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefitL12, prevPrefitL12;
                              double currPrefitC12, prevPrefitC12;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefitL12 = stv.second(TypeID::prefitL12);
                                  prevPrefitL12 = tvDataPrev(TypeID::prefitL12); 

                                  currPrefitC12 = stv.second(TypeID::prefitC12);
                                  prevPrefitC12 = tvDataPrev(TypeID::prefitC12); 

                                  stv.second[TypeID::diffPrefitL12] = currPrefitL12 - prevPrefitL12;
                                  stv.second[TypeID::diffPrefitC12] = currPrefitC12 - prevPrefitC12;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }

                          }
                          else if( type == TypeID::LC15)
                          {
                              bool hasCurrPrefit(false);
                              if( 
                                  stv.second.find(TypeID::prefitL15) != stv.second.end() &&
                                  stv.second.find(TypeID::prefitC15) != stv.second.end() 
                                  )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( 
                                  tvDataPrev.find(TypeID::prefitL15) != tvDataPrev.end() &&
                                  tvDataPrev.find(TypeID::prefitC15) != tvDataPrev.end() 
                                  )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefitL15, prevPrefitL15;
                              double currPrefitC15, prevPrefitC15;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefitL15 = stv.second(TypeID::prefitL15);
                                  prevPrefitL15 = tvDataPrev(TypeID::prefitL15); 

                                  currPrefitC15 = stv.second(TypeID::prefitC15);
                                  prevPrefitC15 = tvDataPrev(TypeID::prefitC15); 

                                  stv.second[TypeID::diffPrefitL15] = currPrefitL15 - prevPrefitL15;
                                  stv.second[TypeID::diffPrefitC15] = currPrefitC15 - prevPrefitC15;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }

                      }
                  }
                  else if(sat.system == SatelliteSystem::Galileo)
                  {
                      for(auto type: galCombTypeSet)
                      {
                          if( type == TypeID::LC15)
                          {
                              bool hasCurrPrefit(false);
                              if( 
                                  stv.second.find(TypeID::prefitL15) != stv.second.end() &&
                                  stv.second.find(TypeID::prefitC15) != stv.second.end() 
                                  )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( 
                                   tvDataPrev.find(TypeID::prefitL15) != tvDataPrev.end() &&
                                   tvDataPrev.find(TypeID::prefitC15) != tvDataPrev.end() 
                                   )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefitL15, prevPrefitL15;
                              double currPrefitC15, prevPrefitC15;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefitL15 = stv.second(TypeID::prefitL15);
                                  prevPrefitL15 = tvDataPrev(TypeID::prefitL15); 

                                  stv.second[TypeID::diffPrefitL15] = currPrefitL15 - prevPrefitL15;

                                  currPrefitC15 = stv.second(TypeID::prefitC15);
                                  prevPrefitC15 = tvDataPrev(TypeID::prefitC15); 
                                  stv.second[TypeID::diffPrefitC15] = currPrefitC15 - prevPrefitC15;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                          else if( type == TypeID::LC17)
                          {
                              bool hasCurrPrefit(false);
                              if( 
                                   stv.second.find(TypeID::prefitL17) != stv.second.end() &&
                                   stv.second.find(TypeID::prefitC17) != stv.second.end() 
                                   )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( 
                                   tvDataPrev.find(TypeID::prefitL17) != tvDataPrev.end() &&
                                   tvDataPrev.find(TypeID::prefitC17) != tvDataPrev.end()
                                      )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefitL17, prevPrefitL17;
                              double currPrefitC17, prevPrefitC17;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefitL17 = stv.second(TypeID::prefitL17);
                                  prevPrefitL17 = tvDataPrev(TypeID::prefitL17); 
                                  stv.second[TypeID::diffPrefitL17] = currPrefitL17 - prevPrefitL17;

                                  currPrefitC17 = stv.second(TypeID::prefitC17);
                                  prevPrefitC17 = tvDataPrev(TypeID::prefitC17); 
                                  stv.second[TypeID::diffPrefitC17] = currPrefitC17 - prevPrefitC17;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                          else if( type == TypeID::LC18)
                          {
                              bool hasCurrPrefit(false);
                              if( 
                                   stv.second.find(TypeID::prefitL18) != stv.second.end() &&
                                   stv.second.find(TypeID::prefitC18) != stv.second.end() 
                                   )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( 
                                  tvDataPrev.find(TypeID::prefitL18) != tvDataPrev.end() &&
                                  tvDataPrev.find(TypeID::prefitC18) != tvDataPrev.end() 
                                      )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefitL18, prevPrefitL18;
                              double currPrefitC18, prevPrefitC18;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefitL18 = stv.second(TypeID::prefitL18);
                                  prevPrefitL18 = tvDataPrev(TypeID::prefitL18); 
                                  stv.second[TypeID::diffPrefitL18] = currPrefitL18 - prevPrefitL18;

                                  currPrefitC18 = stv.second(TypeID::prefitC18);
                                  prevPrefitC18 = tvDataPrev(TypeID::prefitC18); 
                                  stv.second[TypeID::diffPrefitC18] = currPrefitC18 - prevPrefitC18;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                          else if( type == TypeID::LC16)
                          {
                              bool hasCurrPrefit(false);
                              if( 
                                   stv.second.find(TypeID::prefitL16) != stv.second.end() &&
                                   stv.second.find(TypeID::prefitC16) != stv.second.end() 
                                   )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( 
                                  tvDataPrev.find(TypeID::prefitL16) != tvDataPrev.end() &&
                                  tvDataPrev.find(TypeID::prefitC16) != tvDataPrev.end() 
                                      )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefitL16, prevPrefitL16;
                              double currPrefitC16, prevPrefitC16;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefitL16 = stv.second(TypeID::prefitL16);
                                  prevPrefitL16 = tvDataPrev(TypeID::prefitL16); 
                                  stv.second[TypeID::diffPrefitL16] = currPrefitL16 - prevPrefitL16;

                                  currPrefitC16 = stv.second(TypeID::prefitC16);
                                  prevPrefitC16 = tvDataPrev(TypeID::prefitC16); 
                                  stv.second[TypeID::diffPrefitC16] = currPrefitC16 - prevPrefitC16;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                      }
                  }
                  else if(sat.system == SatelliteSystem::BDS)
                  {
                      for(auto type: bdsCombTypeSet)
                      {
                          if( type == TypeID::LC26)
                          {
                              bool hasCurrPrefit(false);
                              if( 
                                   stv.second.find(TypeID::prefitL26) != stv.second.end() &&
                                   stv.second.find(TypeID::prefitC26) != stv.second.end() 
                                   )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( 
                                  tvDataPrev.find(TypeID::prefitL26) != tvDataPrev.end() &&
                                  tvDataPrev.find(TypeID::prefitC26) != tvDataPrev.end() 
                                      )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefitL26, prevPrefitL26;
                              double currPrefitC26, prevPrefitC26;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefitL26 = stv.second(TypeID::prefitL26);
                                  prevPrefitL26 = tvDataPrev(TypeID::prefitL26); 
                                  stv.second[TypeID::diffPrefitL26] = currPrefitL26 - prevPrefitL26;

                                  currPrefitC26 = stv.second(TypeID::prefitC26);
                                  prevPrefitC26 = tvDataPrev(TypeID::prefitC26); 
                                  stv.second[TypeID::diffPrefitC26] = currPrefitC26 - prevPrefitC26;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                          else if( type == TypeID::LC27)
                          {
                              bool hasCurrPrefit(false);
                              if( 
                                   stv.second.find(TypeID::prefitL27) != stv.second.end() &&
                                   stv.second.find(TypeID::prefitC27) != stv.second.end() 
                                   )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( 
                                  tvDataPrev.find(TypeID::prefitL27) != tvDataPrev.end() &&
                                  tvDataPrev.find(TypeID::prefitC27) != tvDataPrev.end() 
                                      )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefitL27, prevPrefitL27;
                              double currPrefitC27, prevPrefitC27;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefitL27 = stv.second(TypeID::prefitL27);
                                  prevPrefitL27 = tvDataPrev(TypeID::prefitL27); 
                                  stv.second[TypeID::diffPrefitL27] = currPrefitL27 - prevPrefitL27;

                                  currPrefitC27 = stv.second(TypeID::prefitC27);
                                  prevPrefitC27 = tvDataPrev(TypeID::prefitC27); 
                                  stv.second[TypeID::diffPrefitC27] = currPrefitC27 - prevPrefitC27;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                      }
                  }
                  else if(sat.system == SatelliteSystem::GLONASS)
                  {
                      // to be done
                  }

              }
              else
              {
                  if(sat.system==SatelliteSystem::GPS)
                  {
                      stv.second[TypeID::CSLC] = 9.0;
                  }
                  else if(sat.system==SatelliteSystem::Galileo)
                  {
                      stv.second[TypeID::CSLC] = 9.0;
                  }
                  else if(sat.system==SatelliteSystem::BDS)
                  {
                      stv.second[TypeID::CSLC] = 9.0;
                  }
                  else if(sat.system==SatelliteSystem::GLONASS)
                  {
                      // to be done
                  }
              }
          }
       }
       catch(Exception& u)
       {
             // Throw an exception if something unexpected happens
          ProcessingException e( getClassName() + ":"
                                 + StringUtils::asString( getIndex() ) + ":"
                                 + u.what() );

          GPSTK_THROW(e);

       }

       return;
   }


}  // End of namespace gpstk
